{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "%matplotlib inline\n",
    "%load_ext autoreload\n",
    "%autoreload 2\n",
    "import sys\n",
    "sys.path.insert(0,'../')\n",
    "from pose_graph_partitioning.pose_graph import *\n",
    "from pose_graph_partitioning.pose_graph_partitioning import *\n",
    "from simulate_utils import ATE, align_posegraph\n",
    "\n",
    "plt.rc(\"figure\", figsize=(10,10))\n",
    "def compare(groundtruth_file=None, results=None, inputs=None, ceres_res=None, show=True,\n",
    "     show_on_same_plot=True, unino_input=True, plot3d=False):\n",
    "    if groundtruth_file is not None:\n",
    "        pg_gt = PoseGraph()\n",
    "        pg_gt.read_g2o_single(groundtruth_file)\n",
    "        align_posegraph(pg_gt)\n",
    "    if inputs is not None:\n",
    "        pg_input = PoseGraph()\n",
    "        pg_input.read_g2o_folder(inputs)\n",
    "        align_posegraph(pg_input)\n",
    "        if groundtruth_file is not None:\n",
    "            ate_T, ate_rot = ATE(pg_input, pg_gt)\n",
    "            print(f\"ATE of input {ate_T:.2f} {ate_rot*57.3:.2f} deg\") \n",
    "        if unino_input:\n",
    "            partitioning(pg_input, \"vol\", agent_num=1, show=False, method=\"union\")\n",
    "    if results is not None:\n",
    "        pg_res = PoseGraph()\n",
    "        pg_res.read_g2o_folder(results)\n",
    "        align_posegraph(pg_res)\n",
    "        if groundtruth_file is not None:\n",
    "            ate_T, ate_rot = ATE(pg_res, pg_gt)\n",
    "            print(f\"ATE of result {ate_T:.2f} {ate_rot*57.3:.2f} deg\") \n",
    "        if unino_input:\n",
    "            partitioning(pg_res, \"vol\", agent_num=1, show=False, method=\"union\")\n",
    "    if ceres_res is not None:\n",
    "        pg_ceres = PoseGraph()\n",
    "        pg_ceres.read_g2o_single(ceres_res)\n",
    "        align_posegraph(pg_ceres)\n",
    "        if groundtruth_file is not None:\n",
    "            ate_T, ate_rot = ATE(pg_gt, pg_ceres, debug=False)\n",
    "            print(f\"ATE of ceres {ate_T:.2f} {ate_rot*57.3:.2f} deg\") \n",
    "    if show:\n",
    "        if inputs is not None:\n",
    "            ax = pg_input.show(\"Input\", clear=False, show_edges=False, plot3d=plot3d)\n",
    "        else:\n",
    "            ax = None\n",
    "        if ceres_res is not None:\n",
    "            if not show_on_same_plot:\n",
    "                ax = None\n",
    "            ax = pg_ceres.show(\"ceres\", ax=ax, clear=False, show_edges=False, plot3d=plot3d, marker=\"+\")\n",
    "        if not show_on_same_plot:\n",
    "            ax = None\n",
    "        if groundtruth_file is not None:\n",
    "            ax = pg_gt.show(\"GT\", ax=ax, show_edges=False, clear=False, plot3d=plot3d)\n",
    "        if not show_on_same_plot:\n",
    "            ax = None\n",
    "        if results:\n",
    "            pg_res.show(\"Res\", ax=ax, clear=False, show_edges=False, plot3d=plot3d)\n",
    "        plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "groundtruth_file = '/home/xuhao/data/d2slam/pgo/parking-garage/groundtruth.g2o'\n",
    "results = '/home/xuhao/data/d2slam/pgo/parking-garage/output'\n",
    "inputs = '/home/xuhao/data/d2slam/pgo/parking-garage/input'\n",
    "ceres_res = None #\"/home/xuhao/data/d2slam/pgo/parking-garage/ceres-output.g2o\"\n",
    "\n",
    "compare(groundtruth_file, results, inputs, ceres_res=ceres_res, show_on_same_plot=True)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "groundtruth_file = '/home/xuhao/data/d2slam/pgo/input_INTEL_g2o/groundtruth.g2o'\n",
    "results = '/home/xuhao/data/d2slam/pgo/input_INTEL_g2o/output'\n",
    "inputs = '/home/xuhao/data/d2slam/pgo/input_INTEL_g2o/input'\n",
    "ceres_res = \"/home/xuhao/data/d2slam/pgo/input_INTEL_g2o/ceres-output.g2o\"\n",
    "compare(groundtruth_file, results, inputs, ceres_res=ceres_res)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "groundtruth_file = '/home/xuhao/data/d2slam/pgo/cubicle/groundtruth.g2o'\n",
    "results = '/home/xuhao/data/d2slam/pgo/cubicle/output'\n",
    "# inputs = '/home/xuhao/data/d2slam/pgo/cubicle/input'\n",
    "# ceres_res = \"/home/xuhao/data/d2slam/pgo/cubicle/ceres-output.g2o\"\n",
    "compare(groundtruth_file, results, inputs, ceres_res=ceres_res)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "plt.rc(\"figure\", figsize=(10,8))\n",
    "groundtruth_file = '/home/xuhao/data/d2slam/pgo/torus3D/groundtruth.g2o'\n",
    "results = '/home/xuhao/data/d2slam/pgo/torus3D/output'\n",
    "inputs = None #'/home/xuhao/data/d2slam/pgo/torus3D/input'\n",
    "ceres_res = None #\"/home/xuhao/data/ 5 rd2slam/pgo/torus3D/ceres-output.g2o\"\n",
    "compare(groundtruth_file, results, inputs, ceres_res=ceres_res, plot3d=True)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "plt.rc(\"figure\", figsize=(20,8))\n",
    "results = '/home/xuhao/data/d2slam/pgo/tum_corr/output'\n",
    "inputs = None #'/home/xuhao/data/d2slam/pgo/tum_corr/input'\n",
    "ceres_res = None #\"/home/xuhao/data/d2slam/pgo/tum_corr/ceres-output.g2o\"\n",
    "groundtruth_file = '/home/xuhao/data/d2slam/pgo/tum_corr/groundtruth.g2o'\n",
    "compare(groundtruth_file, results, inputs, ceres_res=ceres_res, unino_input=False, plot3d=False)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3.9.12 ('base')",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.8.5"
  },
  "orig_nbformat": 4,
  "vscode": {
   "interpreter": {
    "hash": "08ce52785f0fedc81003ce387e097a83d6cc9494681cd746006386992005bb71"
   }
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
